import rclpy
import math
from rclpy.node import Node
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
from chapt4_interfaces.srv import Partol
class TurtleControl(Node):
    def __init__(self,node_name):
        super().__init__(node_name)
        self.get_logger().info(f"{node_name} 启动")
        self.getLocation_sub=self.create_subscription(Pose,"/turtle1/pose",self.pose_callback,10)
        self.trutle_pub=self.create_publisher(Twist,'/turtle1/cmd_vel',10)
        self.target_x=0.0
        self.target_y=0.0
        # 创建服务
        self.service=self.create_service(Partol,'turtle_control',self.service_callback)
    def pose_callback(self,pose:Pose):
        current_x=pose.x
        current_y=pose.y
        
        # 计算当前位置和目标位置的距离差和角度差别
        distance=math.sqrt(
            (self.target_x-current_x)*(self.target_x-current_x)+
            (self.target_y-current_y)*(self.target_y-current_y)
        ) # 需要前进的距离
        
        angle=math.atan2((self.target_y-current_y),(self.target_x-current_x)) -pose.theta # 需要旋转的角度
        k_=1.0
        max_speed=2.0
        # 控制策略
        msg=Twist()
        if distance>0.1:
            if abs(angle)>0.1: # 先旋转
                msg.angular.z=angle
            else:
                msg.linear.x=k_*distance # 再移动
        # 限制线速度最大值
        if msg.linear.x>max_speed:
            #msg.linear.x=max_speed
            pass

        self.trutle_pub.publish(msg)

    def service_callback(self,request,response):
       
        self.get_logger().info(f"传递的参数为: x:{request.target_x} y:{request.target_y}")
        self.target_x=request.target_x
        self.target_y=request.target_y
        # 返回的参数
        response.result=1


        return response #必须返回
def main():
    rclpy.init()
    node=TurtleControl('turtle_control')
    rclpy.spin(node)

    rclpy.shutdown()